//
// Created by augtons on 2022/1/16.
//

#include "espressif_mpu_inc.h"
#include "driver/i2c.h"


#define I2C_NUM     I2C_NUM_0
#define SDA_NUM     18
#define SCL_NUM     19

int esp32_get_ms(unsigned long *count){
    count[0] = esp_timer_get_time() / 1000;
    return 0;
}

int esp32_i2c_readData(unsigned char slave_addr, unsigned char reg_addr,
                          unsigned char length, unsigned char *data)
{
    unsigned char slave_addr_left1 = slave_addr << 1;
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    //0 write
    i2c_master_write_byte(cmd, slave_addr_left1 | 0, true);
    i2c_master_write_byte(cmd, reg_addr, true);
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, slave_addr_left1 | 1, true);
    i2c_master_read(cmd, data, length, I2C_MASTER_LAST_NACK);
    i2c_master_stop(cmd);

    esp_err_t err = i2c_master_cmd_begin(I2C_NUM, cmd, 0);
    i2c_cmd_link_delete(cmd);
    return err;
}

int esp32_i2c_writeDate(unsigned char slave_addr, unsigned char reg_addr,
                          unsigned char length, unsigned char const *data)
{
    int err = 0;
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, slave_addr << 1, true);
    i2c_master_write_byte(cmd, reg_addr, true);
    i2c_master_write(cmd, data, length, true);
    i2c_master_stop(cmd);

    err = i2c_master_cmd_begin(I2C_NUM, cmd, 0);
    i2c_cmd_link_delete(cmd);
    return err;
}

int esp_i2c_install_driver(){
    i2c_config_t cfg = {
        .mode = I2C_MODE_MASTER,
        .scl_io_num = SCL_NUM,
        .sda_io_num = SDA_NUM,
        .scl_pullup_en = true,
        .sda_pullup_en = true,
        .master.clk_speed = 100000,
    };
    if(ESP_OK != i2c_param_config(I2C_NUM, &cfg)){
        ESP_LOGE("MPU_I2C", "param set failed!");
        return -1;
    }
    if(ESP_OK != i2c_driver_install(I2C_NUM, I2C_MODE_MASTER, 128, 128, ESP_INTR_FLAG_SHARED)){
        ESP_LOGE("MPU_I2C", "driver install failed!");
        return -1;
    }
    if(ESP_OK != i2c_set_pin(I2C_NUM_0, SDA_NUM, SCL_NUM, true, true, I2C_MODE_MASTER)){
        ESP_LOGE("MPU_I2C", "failed when setting pin!");
        return -1;
    }
    return 0;
}